/*
 * File: fk6s.c
 *
 * MATLAB Coder version            : 4.1
 * C/C++ source code generated on  : 28-Jul-2020 15:28:14
 */

 /* Include Files */
#include <stdio.h>
#include <stddef.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <time.h> 
//#include "rt_defines.h"
//#include "rt_nonfinite.h"
#include <rocr6_driver/fk6s.h>

#define RT_PI		3.1415926535897931
#define PRECISION	0.001

/* Function Declarations */
static double rt_atan2d_snf(double u0, double u1);

/* Function Definitions */

/*
 * Arguments    : double u0
 *                double u1
 * Return Type  : double
 */
static double rt_atan2d_snf(double u0, double u1)
{
	double y;
	// int b_u0;
	// int b_u1;
	// if (rtIsNaN(u0) || rtIsNaN(u1)) {
	  // y = rtNaN;
	// } else if (rtIsInf(u0) && rtIsInf(u1)) {
	  // if (u0 > 0.0) {
		// b_u0 = 1;
	  // } else {
		// b_u0 = -1;
	  // }

	  // if (u1 > 0.0) {
		// b_u1 = 1;
	  // } else {
		// b_u1 = -1;
	  // }

	  // y = atan2(b_u0, b_u1);
	// } else 

	//printf("%f %f\r\n",u0, u1);

	if (u1 == 0.0) {
		if (u0 > 0.0) {
			y = RT_PI / 2.0;
		}
		else if (u0 < 0.0) {
			y = -(RT_PI / 2.0);
		}
		else {
			y = 0.0;
		}
	}
	else {
		y = atan2(u0, u1);
	}

	return y;
}

/*
 *==========================================
 *		d        a        alpha    offset
 *==========================================
 *      0.144    0        0        0;
 *      0        0        pi/2     -pi/2;
 *      0        -0.264   0        0;
 *      0.106    -0.236   0        -pi/2;
 *      0.114    0        pi/2     0;
 *      0.067    0        -pi/2    0;
 *------------------------------------------
 * Arguments    : const double dt[6]
 *                const double dh[24]
 *                double sp[6]
 * Return Type  : void
 */
void fk6s(const double dt[6], const double dh[24], double sp[16])
{
	double theta1;
	double theta2;
	double theta3;
	double theta4;
	double theta5;
	double theta6;
	double d0;
	double dv0[16];
	double rpy_idx_2;
	double rpy_idx_1;
	double dv1[16];
	int i0;
	int i1;
	int T60_tmp;
	int b_T60_tmp;
	double T60[16];
	theta1 = dh[18] + dt[0];
	theta2 = dh[19] + dt[1];
	theta3 = dh[20] + dt[2];
	theta4 = dh[21] + dt[3];
	theta5 = dh[22] + dt[4];
	theta6 = dh[23] + dt[5];

	/* disp('�����˻�������ϵ��ĩ��ִ��������ı任����T60Ϊ:') */
	d0 = cos(theta1);
	dv0[0] = d0;
	rpy_idx_2 = sin(theta1);
	dv0[4] = -rpy_idx_2;
	dv0[8] = 0.0;
	dv0[12] = dh[6];
	rpy_idx_1 = cos(dh[12]);
	dv0[1] = rpy_idx_2 * rpy_idx_1;
	dv0[5] = d0 * rpy_idx_1;
	theta1 = sin(dh[12]);
	dv0[9] = -theta1;
	dv0[13] = -dh[0] * theta1;
	dv0[2] = rpy_idx_2 * theta1;
	dv0[6] = d0 * theta1;
	dv0[10] = rpy_idx_1;
	dv0[14] = dh[0] * rpy_idx_1;
	d0 = cos(theta2);
	dv1[0] = d0;
	rpy_idx_2 = sin(theta2);
	dv1[4] = -rpy_idx_2;
	dv1[8] = 0.0;
	dv1[12] = dh[7];
	rpy_idx_1 = cos(dh[13]);
	dv1[1] = rpy_idx_2 * rpy_idx_1;
	dv1[5] = d0 * rpy_idx_1;
	theta1 = sin(dh[13]);
	dv1[9] = -theta1;
	dv1[13] = -dh[1] * theta1;
	dv1[2] = rpy_idx_2 * theta1;
	dv1[6] = d0 * theta1;
	dv1[10] = rpy_idx_1;
	dv1[14] = dh[1] * rpy_idx_1;
	dv0[3] = 0.0;
	dv1[3] = 0.0;
	dv0[7] = 0.0;
	dv1[7] = 0.0;
	dv0[11] = 0.0;
	dv1[11] = 0.0;
	dv0[15] = 1.0;
	dv1[15] = 1.0;
	for (i0 = 0; i0 < 4; i0++) {
		for (i1 = 0; i1 < 4; i1++) {
			T60_tmp = i1 << 2;
			b_T60_tmp = i0 + T60_tmp;
			T60[b_T60_tmp] = 0.0;
			T60[b_T60_tmp] = ((dv0[i0] * dv1[T60_tmp] + dv0[i0 + 4] * dv1[1 + T60_tmp])
				+ dv0[i0 + 8] * dv1[2 + T60_tmp]) + dv0[i0 + 12] * dv1[3 + T60_tmp];
		}
	}

	d0 = cos(theta3);
	dv0[0] = d0;
	rpy_idx_2 = sin(theta3);
	dv0[4] = -rpy_idx_2;
	dv0[8] = 0.0;
	dv0[12] = dh[8];
	rpy_idx_1 = cos(dh[14]);
	dv0[1] = rpy_idx_2 * rpy_idx_1;
	dv0[5] = d0 * rpy_idx_1;
	theta1 = sin(dh[14]);
	dv0[9] = -theta1;
	dv0[13] = -dh[2] * theta1;
	dv0[2] = rpy_idx_2 * theta1;
	dv0[6] = d0 * theta1;
	dv0[10] = rpy_idx_1;
	dv0[14] = dh[2] * rpy_idx_1;
	dv0[3] = 0.0;
	dv0[7] = 0.0;
	dv0[11] = 0.0;
	dv0[15] = 1.0;
	for (i0 = 0; i0 < 4; i0++) {
		for (i1 = 0; i1 < 4; i1++) {
			T60_tmp = i1 << 2;
			b_T60_tmp = i0 + T60_tmp;
			dv1[b_T60_tmp] = 0.0;
			dv1[b_T60_tmp] = ((T60[i0] * dv0[T60_tmp] + T60[i0 + 4] * dv0[1 + T60_tmp])
				+ T60[i0 + 8] * dv0[2 + T60_tmp]) + T60[i0 + 12] * dv0[3 + T60_tmp];
		}
	}

	d0 = cos(theta4);
	dv0[0] = d0;
	rpy_idx_2 = sin(theta4);
	dv0[4] = -rpy_idx_2;
	dv0[8] = 0.0;
	dv0[12] = dh[9];
	rpy_idx_1 = cos(dh[15]);
	dv0[1] = rpy_idx_2 * rpy_idx_1;
	dv0[5] = d0 * rpy_idx_1;
	theta1 = sin(dh[15]);
	dv0[9] = -theta1;
	dv0[13] = -dh[3] * theta1;
	dv0[2] = rpy_idx_2 * theta1;
	dv0[6] = d0 * theta1;
	dv0[10] = rpy_idx_1;
	dv0[14] = dh[3] * rpy_idx_1;
	dv0[3] = 0.0;
	dv0[7] = 0.0;
	dv0[11] = 0.0;
	dv0[15] = 1.0;
	for (i0 = 0; i0 < 4; i0++) {
		for (i1 = 0; i1 < 4; i1++) {
			T60_tmp = i1 << 2;
			b_T60_tmp = i0 + T60_tmp;
			T60[b_T60_tmp] = 0.0;
			T60[b_T60_tmp] = ((dv1[i0] * dv0[T60_tmp] + dv1[i0 + 4] * dv0[1 + T60_tmp])
				+ dv1[i0 + 8] * dv0[2 + T60_tmp]) + dv1[i0 + 12] * dv0[3 + T60_tmp];
		}
	}

	d0 = cos(theta5);
	dv0[0] = d0;
	rpy_idx_2 = sin(theta5);
	dv0[4] = -rpy_idx_2;
	dv0[8] = 0.0;
	dv0[12] = dh[10];
	rpy_idx_1 = cos(dh[16]);
	dv0[1] = rpy_idx_2 * rpy_idx_1;
	dv0[5] = d0 * rpy_idx_1;
	theta1 = sin(dh[16]);
	dv0[9] = -theta1;
	dv0[13] = -dh[4] * theta1;
	dv0[2] = rpy_idx_2 * theta1;
	dv0[6] = d0 * theta1;
	dv0[10] = rpy_idx_1;
	dv0[14] = dh[4] * rpy_idx_1;
	dv0[3] = 0.0;
	dv0[7] = 0.0;
	dv0[11] = 0.0;
	dv0[15] = 1.0;
	for (i0 = 0; i0 < 4; i0++) {
		for (i1 = 0; i1 < 4; i1++) {
			T60_tmp = i1 << 2;
			b_T60_tmp = i0 + T60_tmp;
			dv1[b_T60_tmp] = 0.0;
			dv1[b_T60_tmp] = ((T60[i0] * dv0[T60_tmp] + T60[i0 + 4] * dv0[1 + T60_tmp])
				+ T60[i0 + 8] * dv0[2 + T60_tmp]) + T60[i0 + 12] * dv0[3 + T60_tmp];
		}
	}

	d0 = cos(theta6);
	dv0[0] = d0;
	rpy_idx_2 = sin(theta6);
	dv0[4] = -rpy_idx_2;
	dv0[8] = 0.0;
	dv0[12] = dh[11];
	rpy_idx_1 = cos(dh[17]);
	dv0[1] = rpy_idx_2 * rpy_idx_1;
	dv0[5] = d0 * rpy_idx_1;
	theta1 = sin(dh[17]);
	dv0[9] = -theta1;
	dv0[13] = -dh[5] * theta1;
	dv0[2] = rpy_idx_2 * theta1;
	dv0[6] = d0 * theta1;
	dv0[10] = rpy_idx_1;
	dv0[14] = dh[5] * rpy_idx_1;
	dv0[3] = 0.0;
	dv0[7] = 0.0;
	dv0[11] = 0.0;
	dv0[15] = 1.0;
	for (i0 = 0; i0 < 4; i0++) {
		for (i1 = 0; i1 < 4; i1++) {
			T60_tmp = i1 << 2;
			b_T60_tmp = i0 + T60_tmp;
			T60[b_T60_tmp] = 0.0;
			T60[b_T60_tmp] = ((dv1[i0] * dv0[T60_tmp] + dv1[i0 + 4] * dv0[1 + T60_tmp])
				+ dv1[i0 + 8] * dv0[2 + T60_tmp]) + dv1[i0 + 12] * dv0[3 + T60_tmp];
		}
	}

	/*  x */
	//sp[0] = T60[12];

	/*  y */
	//sp[1] = T60[13];

	/*  z */
	//sp[2] = T60[14];

	//for (int i=0; i<16; i++)
	//{
		//printf("% f ", T60[i]);
	//}
	//printf("\r\n");
	
	if(fabs(T60[8]) > 0.001) {
		/*  when |R13| == 1 */
		/*  singularity */
		theta1 = 0.0;

		/*  roll is zero */
		if (T60[8] > 0.001) {
			rpy_idx_2 = rt_atan2d_snf(T60[6], T60[5]);
			/*  R+Y */
			//printf("R+Y\r\n");
		}
		else {
			rpy_idx_2 = -rt_atan2d_snf(T60[1], T60[2]);
			/*  R-Y */
			//printf("R-Y\r\n");
		}

		rpy_idx_1 = asin(T60[8]);
		//printf("mode 1\r\n");
	}
	else {
		theta1 = -rt_atan2d_snf(T60[4], T60[0]);
		rpy_idx_2 = -rt_atan2d_snf(T60[9], T60[10]);
		rpy_idx_1 = atan(T60[8] * cos(theta1) / T60[0]);
		//printf("mode 2\r\n");
	}

	/*  R */
	//sp[3] = theta1;

	/*  P */
	//sp[4] = rpy_idx_1;

	/*  Y */
	//sp[5] = rpy_idx_2;

    for(int fx=0; fx<16; fx++)
	{
		sp[fx] = T60[fx]; 
	}
}

#ifdef TEST
double dh[] =
{
	0.144,		0,			0,			0.106,			0.114,			0.067,
	0,			0,			-0.264,		-0.236,			0,				0,
	0,			RT_PI / 2,	0,			0,				RT_PI / 2,		-RT_PI / 2,
	0,			-RT_PI / 2,	0,			-RT_PI / 2,		0,				0
};
double dt[6] = { 1.571,1.571,0,0,0,0 };
double sp[6] = { 0,0,0,0,0,0 };

int main(int argc, char* argv[])
{
	printf("\r\n--------\r\n");
	fk6s(dt, dh, sp);
	printf("x=%0.4f	y=%0.4f	z=%0.4f	R=%0.4f	P=%0.4f	Y=%0.4f\r\n", sp[0], sp[1], sp[2], sp[3], sp[4], sp[5]);
	printf("--------\r\n");
}
#endif

/*
 * File trailer for fk6s.c
 *
 * [EOF]
 */
